
#ifndef __MLS_RAW_H__
#define __MLS_RAW_H__

#include <vector>
#include <cmath>

#define MIN(a,b) ((a)>(b)?(b):(a))
#define MAX(a,b) ((a)<(b)?(b):(a))
#define SIGN(a)   (a<0)?(-1):(1)

/// constant values for the cubic equation solver
const float K1DIV3   = 0.3333333;    /// 1/3
const float K1DIV9   = 0.1111111;    /// 1/9
const float K1DIV54  = 0.0185185;    /// 1/54
const float EPSILON  = 1.0E-5;      

using namespace std;

#define MLSRIGID 0

/// Multiplies matrices A and B: C = A*B
void multiply(const float A[3][3], const float B[3][3], float C[3][3]) {
   
   for (int i=0; i<3; ++i)
      for (int j=0; j<3; ++j) {
         C[i][j] = 0;
         for (int k=0; k<3; ++k)
            C[i][j] += A[i][k]*B[k][j];
      }
}

/// Returns one cubic root of cubic equation x^3 + ax^2 + bx + c = 0
float cubicroot(const float a, const float b, const float c) {
   
   const float Q = (3.0*b - a*a)*K1DIV9;
   const float R = (9.0*a*b - 27.0*c - 2.0*a*a*a)*K1DIV54;
   const float D = Q*Q*Q + R*R;
   const float sqrtD = sqrt(D);
   
   if (D>=0.0) {  /// one real, two complex :: D==0 root reals, at least two are equal 
      const float S = SIGN(R+sqrtD)*pow(abs(R+sqrtD), K1DIV3);
      const float T = SIGN(R-sqrtD)*pow(abs(R-sqrtD), K1DIV3);
      return -a*K1DIV3 + S + T;
   }
   else {   /// all root are real and unequal
      const float theta = acos(R/sqrt(-Q*Q*Q));
      return 2.0*sqrt(-Q)*cos(theta*K1DIV3)-a*K1DIV3;
   }
}

inline void cross(const float A[], const float B[], float C[]) {
   C[0] = A[1]*B[2]-B[1]*A[2];
   C[1] = B[0]*A[2]-A[0]*B[2];
   C[2] = A[0]*B[1]-B[0]*A[1];
}

inline float dot(const float A[], const float B[]) {
   return A[0]*B[0] + A[1]*B[1] + A[2]*B[2];
}

float largestRoot(const float M0[], const float M1[], const float M2[]) {
   
   const float detM = M0[0]*(M1[1]*M2[2]-M1[2]*M2[1])+
                      M0[1]*(M1[2]*M2[0]-M1[0]*M2[2])+
                      M0[2]*(M1[0]*M2[1]-M2[0]*M1[1]);
   
   /// set the cubic equation
   float normaM[3], normaM2=0.0, prodNorma2=0.0;
   normaM[0] = dot(M0,M0);
   normaM2 += normaM[0];
   normaM[1] = dot(M1,M1);
   normaM2 += normaM[1];
   normaM[2] = dot(M2,M2);
   normaM2 += normaM[2];
   
   prodNorma2 += normaM[0]*normaM[1];
   prodNorma2 += normaM[1]*normaM[2];
   prodNorma2 += normaM[2]*normaM[0];
   
   float prodesc, prodesc2=0.0;
   prodesc = dot(M0,M1);
   prodesc2 += prodesc*prodesc;
   prodesc = dot(M1,M2);
   prodesc2 += prodesc*prodesc;
   prodesc = dot(M2,M0);
   prodesc2 += prodesc*prodesc;
   
   const float a = -4.0*normaM2;
   const float b = 16.0*(prodNorma2-prodesc2);
   const float c = -64.0*detM*detM;
   /// get the cubic root
   float p2 = cubicroot(a,b,c);
   const float _p = sqrt(p2);
   const float minusp2_plus_4M2 = -p2 + 4.0*normaM2;
   const float _16detMdivp = (16.0*detM)/_p;
   
   /// the largest quartic root
   float maxroot = MAX(-_p + sqrt(minusp2_plus_4M2 - _16detMdivp), 
                        _p + sqrt(minusp2_plus_4M2 + _16detMdivp));
   
   return maxroot*0.5;
}

void rotationVector(const float M0[], const float M1[], const float M2[], const float eigenvalue, float u[]) {
   
   /// solving the 3x3 linear system: (N-eigenvalue*I) e = V
   // the right side
   float V[3]; 
   V[0] = M1[2]-M2[1]; V[1] = M2[0]-M0[2]; V[2] = M0[1]-M1[0];
   // the left side
   float N0[3], N1[3], N2[3];
   N0[0]=M0[0]+M0[0]; N0[1]=M0[1]+M1[0]; N0[2]=M0[2]+M2[0];
   N1[0]=M1[0]+M0[1]; N1[1]=M1[1]+M1[1]; N1[2]=M1[2]+M2[1];
   N2[0]=M2[0]+M0[2]; N2[1]=M2[1]+M1[2]; N2[2]=M2[2]+M2[2];
   N0[0] -= eigenvalue;
   N1[1] -= eigenvalue;
   N2[2] -= eigenvalue;
   
   /// Solve the 3x3 system
   const float sigma = (N0[0]*N2[1] - N2[0]*N0[1])/(N0[0]*N1[1] - N1[0]*N0[1]);
   const float vez = (N0[0]*V[2]  -N2[0]*V[0]  -sigma*(N0[0]*V[1] - N1[0]*V[0]))/
                     (N0[0]*N2[2] -N2[0]*N0[2]-sigma*(N0[0]*N1[2]-N1[0]*N0[2]));
   const float vey = ((N0[0]*V[1] -N1[0]*V[0])-(N0[0]*N1[2]-N1[0]*N0[2])*vez)/
                     (N0[0]*N1[1] -N1[0]*N0[1]);
   const float vex = (V[0]-N0[2]*vez-N0[1]*vey)/N0[0];
   
   u[0] = vex; u[1] = vey; u[2] = vez;
}

float weight(const float p[], const Point3 &Pi) {
   
   float vd[3] = {Pi[0]-p[0], Pi[1]-p[1], Pi[2]-p[2]};
   const float d = sqrt(dot(vd,vd));
   if (d==0) return 1e8;
   return 1.0/(d*d); // 1/|d|^2
}

void weightedCentroids(const float p[], float pstar[], float qstar[], 
                       const vector<Point3> &P, const vector<Point3> &Q) {
   
   const unsigned int NCTRLPOINTS = P.size();
   float Wt=0.0, W;
   pstar[0]=pstar[1]=pstar[2]=0.0;
   qstar[0]=qstar[1]=qstar[2]=0.0;
   for (unsigned int i=0; i<NCTRLPOINTS; i++) {
      W = weight(p,P[i]);
      Wt += W;
      pstar[0] += W*P[i][0];
      pstar[1] += W*P[i][1];
      pstar[2] += W*P[i][2];
      
      qstar[0] += W*Q[i][0];
      qstar[1] += W*Q[i][1];
      qstar[2] += W*Q[i][2];
   }
   pstar[0] = pstar[0]/Wt;   pstar[1] = pstar[1]/Wt;   pstar[2] = pstar[2]/Wt;
   qstar[0] = qstar[0]/Wt;   qstar[1] = qstar[1]/Wt;   qstar[2] = qstar[2]/Wt;
}

void correlationMatrix(const float p[], const float pstar[], const float qstar[],
                       const vector<Point3> &P, const vector<Point3> &Q,
                       float M0[], float M1[], float M2[], float &dotA) {
   
   const unsigned int NCTRLPOINTS = P.size();
   float pjhat[3], qjhat[3];
   M0[0]=M0[1]=M0[2]=0.0;
   M1[0]=M1[1]=M1[2]=0.0;
   M2[0]=M2[1]=M2[2]=0.0;
   dotA = 0.0;
   for (unsigned int j=0; j<NCTRLPOINTS; j++) {
      float W = weight(p,P[j]);
      pjhat[0] = P[j][0]-pstar[0];  pjhat[1] = P[j][1]-pstar[1]; pjhat[2] = P[j][2]-pstar[2];
      qjhat[0] = Q[j][0]-qstar[0];  qjhat[1] = Q[j][1]-qstar[1]; qjhat[2] = Q[j][2]-qstar[2];
      
      M0[0] += W*qjhat[0]*pjhat[0]; M0[1] += W*qjhat[0]*pjhat[1]; M0[2] += W*qjhat[0]*pjhat[2];
      M1[0] += W*qjhat[1]*pjhat[0]; M1[1] += W*qjhat[1]*pjhat[1]; M1[2] += W*qjhat[1]*pjhat[2];
      M2[0] += W*qjhat[2]*pjhat[0]; M2[1] += W*qjhat[2]*pjhat[1]; M2[2] += W*qjhat[2]*pjhat[2];
      dotA += W*(pjhat[0]*pjhat[0] + pjhat[1]*pjhat[1] + pjhat[2]*pjhat[2]);
   }
}

void mlsEval(const vector<Point3> &P, const vector<Point3> &Q, 
   float p[], float pd[], float n[], float nd[]) {
   
   /// compute weighted centroids
   float pstar[3], qstar[3];
   weightedCentroids(p, pstar, qstar, P, Q);
   
   /// compute the correlation matrix
   float M0[3], M1[3], M2[3]; // the matrix rows
   float dotA;
   correlationMatrix(p, pstar, qstar, P, Q, M0, M1, M2, dotA);
   
   /// compute the largest root of polynomial and the eigenvalue
   const float traceM = M0[0] + M1[1] + M2[2];
   const float lroot = largestRoot(M0,M1,M2);
   const float eigenvalue = lroot + traceM;
   
   /// compute the rotation vector
   float u[3];
   rotationVector(M0,M1,M2, eigenvalue, u);
   
   /// modify the vertex position (apply rotation defined by u)
   float vhat[3] = {p[0]-pstar[0], p[1]-pstar[1], p[2]-pstar[2]};
   
   /// rotate 
   float tmpp1[3], tmpp2[3];
   cross(u,vhat,tmpp1);
   cross(tmpp1,u,tmpp2);
   const float numpx = 2.0*(tmpp2[0] - tmpp1[0]);
   const float numpy = 2.0*(tmpp2[1] - tmpp1[1]);
   const float numpz = 2.0*(tmpp2[2] - tmpp1[2]);
   const float denom = 1.0 + (u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
   const float vdx = vhat[0] - numpx/denom;
   const float vdy = vhat[1] - numpy/denom;
   const float vdz = vhat[2] - numpz/denom;
   
   /// rotate the vertex normal 
   float tmpn1[3], tmpn2[3];
   cross(u,n,tmpn1);
   cross(tmpn1,u,tmpn2);
   const float numnx = 2.0*(tmpn2[0] - tmpn1[0]);
   const float numny = 2.0*(tmpn2[1] - tmpn1[1]);
   const float numnz = 2.0*(tmpn2[2] - tmpn1[2]);
   const float ndx = n[0] - numnx/denom;
   const float ndy = n[1] - numny/denom;
   const float ndz = n[2] - numnz/denom;
   
   #if MLSRIGID
   pd[0] = qstar[0] + vdx;
   pd[1] = qstar[1] + vdy;
   pd[2] = qstar[2] + vdz;
   #else // using similarity transformations
   pd[0] = qstar[0] + vdx*(lroot/dotA);
   pd[1] = qstar[1] + vdy*(lroot/dotA);
   pd[2] = qstar[2] + vdz*(lroot/dotA);
   #endif
   nd[0] = ndx;
   nd[1] = ndy;
   nd[2] = ndz;
}

#endif
